# -*- coding: utf-8 -*-
"""
Created on Fri Dec 24 20:03:32 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Medium_formulate_Opt import *
from Obtain_results import *

def M_MPC(initial_state, state_bnds_lo, state_bnds_up, 
          slow_state_bnds, 
          fast_state_bnds_lo, fast_state_bnds_up, 
          fast_state_slack_lo, fast_state_slack_up, 
          slow_input_bnds, 
          input_guess, input_bnds_lo, input_bnds_up, 
          fast_input_bnds_lo, fast_input_bnds_up, 
          input_bin_bnds,
          distb_bnds,
          output_guess, 
          output_setpnts, medium_state_setpnts, fast_state_setpnts, 
          medium_input_setpnts, fast_input_setpnts, 
          alpha, alpha_xm, alpha_xf, alpha_um, alpha_uf, alpha_slack, pred_horzn, i):
    
    initial_state_new = np.append(initial_state[3:9], 
                                  np.array([initial_state[11], initial_state[14], 
                                            initial_state[21]]))
    state_guess_new = initial_state_new
    fast_state_guess_new = np.delete(initial_state, 
                                     np.array((3,4,5,6,7,8,11,14,16,18,19,20,21,22)))
    input_guess_new = input_guess[1]
    fast_input_guess_new = np.array([input_guess[0], input_guess[3]])
    distb_bnds_new = distb_bnds[i:i+pred_horzn, :]
    output_setpnts_new = output_setpnts[i:i+pred_horzn]
    
    f_opt = formulate_opt_m(initial_state_new, state_guess_new, state_bnds_lo, state_bnds_up, 
                            slow_state_bnds, 
                            fast_state_guess_new, fast_state_bnds_lo, fast_state_bnds_up, 
                            fast_state_slack_lo, fast_state_slack_up, 
                            slow_input_bnds, 
                            input_guess_new, input_bnds_lo, input_bnds_up, 
                            fast_input_guess_new, fast_input_bnds_lo, fast_input_bnds_up, 
                            input_bin_bnds,
                            distb_bnds_new,
                            output_guess, 
                            output_setpnts_new, medium_state_setpnts, fast_state_setpnts, 
                            medium_input_setpnts, fast_input_setpnts, 
                            alpha, alpha_xm, alpha_xf, alpha_um, alpha_uf, 
                            alpha_slack, pred_horzn)
    
    s_opt = solve_opt_m(*f_opt)
    
    r_opt = get_results_m(s_opt, pred_horzn)
    
    return r_opt
